package rollercoaster.train;

import rollercoaster.commons.Vertex3D;
import rollercoaster.commons.interfaces.Surface3D;
import rollercoaster.railway.utils.VertexUtils;

public class UniformClosedSecondOrderPatch implements Surface3D {
	
	private Vertex3D[][] controlPoints;
	
	private float new_u;
	private float new_v;
	
	private Vertex3D[][] subMatrix = new Vertex3D[3][3];

	public UniformClosedSecondOrderPatch(Vertex3D[][] controlPoints) {
		super();
		this.controlPoints = controlPoints;
	}

	public Vertex3D[][] getControlPoints() {
		return controlPoints;
	}

	public void setControlPoints(Vertex3D[][] controlPoints) {
		this.controlPoints = controlPoints;
	}

	public Vertex3D sigma(float u, float v) {
		focusOnSubmatrix(u,v);
		
		Vertex3D[] gammas=new Vertex3D[3];
		float gamma_ix;
		float gamma_iy;
		float gamma_iz;
		Vertex3D start;
		Vertex3D end;
		
		for (int i = 0; i < 3; i++) {
			
			start=VertexUtils.meanPoint(subMatrix[i][0], subMatrix[i][1]);
			end=VertexUtils.meanPoint(subMatrix[i][1], subMatrix[i][2]);
			
			gamma_ix=(1-new_u)*(1-new_u)*start.getX() + 2*new_u*(1-new_u)*subMatrix[i][1].getX() + new_u*new_u*end.getX();
			gamma_iy=(1-new_u)*(1-new_u)*start.getY() + 2*new_u*(1-new_u)*subMatrix[i][1].getY() + new_u*new_u*end.getY();
			gamma_iz=(1-new_u)*(1-new_u)*start.getZ() + 2*new_u*(1-new_u)*subMatrix[i][1].getZ() + new_u*new_u*end.getZ();
			
			gammas[i]=new Vertex3D(gamma_ix, gamma_iy, gamma_iz);
			
			//System.out.println(gammas[i]);
		}
		
		start=VertexUtils.meanPoint(gammas[0], gammas[1]);
		end=VertexUtils.meanPoint(gammas[1], gammas[2]);
		
		float sigma_x=(1-new_v)*(1-new_v)*start.getX() + 2*new_v*(1-new_v)*gammas[1].getX() + new_v*new_v*end.getX();
		float sigma_y=(1-new_v)*(1-new_v)*start.getY() + 2*new_v*(1-new_v)*gammas[1].getY() + new_v*new_v*end.getY();
		float sigma_z=(1-new_v)*(1-new_v)*start.getZ() + 2*new_v*(1-new_v)*gammas[1].getZ() + new_v*new_v*end.getZ();
		
		Vertex3D sigma=new Vertex3D(sigma_x, sigma_y, sigma_z);
		
		return sigma;
	}

	public Vertex3D dSigmadu(float u, float v) {
		
		focusOnSubmatrix(u, v);

		Vertex3D[] gPrimes=new Vertex3D[3];
		
		float gPrime_ix;
		float gPrime_iy;
		float gPrime_iz;
		
		Vertex3D start;
		Vertex3D end;
		
		
		for (int i = 0; i < 3; i++) {
			
			start = VertexUtils.meanPoint(subMatrix[i][0], subMatrix[i][1]);
			end = VertexUtils.meanPoint(subMatrix[i][1], subMatrix[i][2]);
			
			gPrime_ix = -2*(1-new_u)*start.getX()+2*(1-new_u)*subMatrix[i][1].getX()-2*new_u*subMatrix[i][1].getX()+2*new_u*end.getX();
			gPrime_iy = -2*(1-new_u)*start.getY()+2*(1-new_u)*subMatrix[i][1].getY()-2*new_u*subMatrix[i][1].getY()+2*new_u*end.getY();
			gPrime_iz = -2*(1-new_u)*start.getZ()+2*(1-new_u)*subMatrix[i][1].getZ()-2*new_u*subMatrix[i][1].getZ()+2*new_u*end.getZ();
			
			gPrimes[i]= new Vertex3D(gPrime_ix, gPrime_iy, gPrime_iz);
			
		}
		
		start = VertexUtils.meanPoint(gPrimes[0], gPrimes[1]);
		end = VertexUtils.meanPoint(gPrimes[1], gPrimes[2]);
		
		float dSigmadUX =(1-new_v)*(1-new_v)*start.getX() + 2*new_v*(1-new_v)*gPrimes[1].getX() + new_v*new_v*end.getX();
		float dSigmadUY =(1-new_v)*(1-new_v)*start.getY() + 2*new_v*(1-new_v)*gPrimes[1].getY() + new_v*new_v*end.getY();
		float dSigmadUZ =(1-new_v)*(1-new_v)*start.getZ() + 2*new_v*(1-new_v)*gPrimes[1].getZ() + new_v*new_v*end.getZ();
		
		Vertex3D dSigmadU = new Vertex3D(dSigmadUX, dSigmadUY, dSigmadUZ);
		
		return dSigmadU;
	}

	public Vertex3D dSigmadv(float u, float v) {
		focusOnSubmatrix(u, v);
		
		Vertex3D[] gammas=new Vertex3D[3];
		float gamma_ix;
		float gamma_iy;
		float gamma_iz;
		Vertex3D start;
		Vertex3D end;
		
		for (int i = 0; i < 3; i++) {
			
			start=VertexUtils.meanPoint(subMatrix[i][0], subMatrix[i][1]);
			end=VertexUtils.meanPoint(subMatrix[i][1], subMatrix[i][2]);
			
			gamma_ix=(1-new_u)*(1-new_u)*start.getX() + 2*new_u*(1-new_u)*subMatrix[i][1].getX() + new_u*new_u*end.getX();
			gamma_iy=(1-new_u)*(1-new_u)*start.getY() + 2*new_u*(1-new_u)*subMatrix[i][1].getY() + new_u*new_u*end.getY();
			gamma_iz=(1-new_u)*(1-new_u)*start.getZ() + 2*new_u*(1-new_u)*subMatrix[i][1].getZ() + new_u*new_u*end.getZ();
			
			gammas[i]=new Vertex3D(gamma_ix, gamma_iy, gamma_iz);
			
		}
		
		start=VertexUtils.meanPoint(gammas[0], gammas[1]);
		end=VertexUtils.meanPoint(gammas[1], gammas[2]);
		
		float dSigmadVX=-2*(1-new_v)*start.getX()+2*(1-new_v)*gammas[1].getX()-2*new_v*gammas[1].getX()+2*new_v*end.getX();
		float dSigmadVY=-2*(1-new_v)*start.getY()+2*(1-new_v)*gammas[1].getY()-2*new_v*gammas[1].getY()+2*new_v*end.getY();
		float dSigmadVZ=-2*(1-new_v)*start.getZ()+2*(1-new_v)*gammas[1].getZ()-2*new_v*gammas[1].getZ()+2*new_v*end.getZ();
		
		Vertex3D dSigmadV = new Vertex3D(dSigmadVX, dSigmadVY, dSigmadVZ);
		
		return dSigmadV;
	}

	public Vertex3D getNormal(float u, float v) {
		Vertex3D dSigmadU=dSigmadu(u, v);
		Vertex3D dSigmadV=dSigmadv(u, v);
		Vertex3D normalVector = VertexUtils.crossProduct(dSigmadU, dSigmadV);
		VertexUtils.normalizeVector(normalVector);
		return normalVector;
	}

	public float getStartUPoint() {
		return 0;
	}

	public float getEndUPoint() {
		return controlPoints[0].length;
	}

	public float getStartVPoint() {
		return 0;
	}

	public float getEndVPoint() {
		return controlPoints.length;
	}
	
	private float normalize(float dim, int length) {

		while(dim<0){
			dim+=length;
		}
		while(dim>=length){
			dim-=length;
		}
		return dim;
	}
	
	private void focusOnSubmatrix(float u, float v){
		int lengthU=controlPoints[0].length;
		int lengthV=controlPoints.length;
		u=normalize(u, lengthU);
		v=normalize(v, lengthV);
		int interval_u=(int) u;
		int interval_v=(int) v;
		new_u=u-interval_u;
		new_v=v-interval_v;

		//P00=controlPoints[interval_v%lengthV][interval_u%lengthU];
		for (int i = 0; i < 3; i++) {
			for (int j = 0; j < 3; j++) {
				subMatrix[i][j]=controlPoints[(interval_v+i)%lengthV][(interval_u+j)%lengthU];
			}
		}
	
	
	}

}
